#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
import math

ev3 = EV3Brick()
moterA = Motor(Port.A)
moterB = Motor(Port.B)
moterC = Motor(Port.C)
moterD = Motor(Port.D)

r_x = 98
r_y = 65
R = 24
my_but = InfraredSensor(Port.S1)  #遥控
gyrosensor = GyroSensor(Port.S2)  #陀螺仪
mytouchsensor = TouchSensor(Port.S3)  #触碰传感器
myultra = UltrasonicSensor(Port.S4)
mytime = StopWatch()
# 机器人坐标系
def kenetics(v_x,v_y,omega):
    omega_1 = v_x-v_y-(r_x+r_y)*omega/R
    omega_2 = v_x+v_y-(r_x+r_y)*omega/R
    omega_3 = v_x-v_y+(r_x+r_y)*omega/R
    omega_4 = v_x+v_y+(r_x+r_y)*omega/R
    output_vec = [-omega_1,omega_2,omega_3,-omega_4] #电机硬件反向，加负号
    return output_vec
# 地面坐标系
def ground_kenetics(v_x,v_y,omega,angle):
    angle = angle*math.pi/180
    omega_1 = v_x*(math.cos(angle)+math.sin(angle))+v_y*(math.sin(angle)-math.cos(angle))-(r_x+r_y)*omega/R
    omega_2 = v_x*(math.cos(angle)-math.sin(angle))+v_y*(math.sin(angle)+math.cos(angle))-(r_x+r_y)*omega/R
    omega_3 = v_x*(math.cos(angle)+math.sin(angle))+v_y*(math.sin(angle)-math.cos(angle))+(r_x+r_y)*omega/R
    omega_4 = v_x*(math.cos(angle)-math.sin(angle))+v_y*(math.sin(angle)+math.cos(angle))+(r_x+r_y)*omega/R
    output_vec = [-omega_1,omega_2,omega_3,-omega_4]
    return output_vec

v_x = 0
v_y = 0
omega = 0
vel = 250

while True:
    state = my_but.buttons(1)  #读取遥控状态
    touch_state = mytouchsensor.pressed()  #读取触碰状态
    if state == [Button.LEFT_UP]:
        v_x = vel
        v_y = 0
        omega = 0
    elif state == [Button.LEFT_DOWN]:
        v_x = -vel
        v_y = 0
        omega = 0
    elif state == [Button.RIGHT_UP]:
        v_x = 0
        v_y = vel
        omega = 0
    elif state == [Button.RIGHT_DOWN]:
        v_x = 0
        v_y = -vel
        omega = 0

    state = my_but.buttons(2)
    if state == [Button.LEFT_UP]:
        v_x = 0
        v_y = 0
        omega = vel/2
    elif state == [Button.LEFT_DOWN]:
        v_x = 0
        v_y = 0
        omega = -vel/2
    elif state == [Button.RIGHT_UP] or touch_state:  # 遥控或触碰
        v_x = 0
        v_y = 0
        omega = 0
    elif state == [Button.RIGHT_DOWN] or touch_state:
        v_x = 0
        v_y = 0
        omega = 0

    output_vec = kenetics(v_x,v_y,omega)
    # output_vec = ground_kenetics(v_x,v_y,omega,angle)
    moterA.run(output_vec[0]) 
    moterB.run(output_vec[1]) 
    moterC.run(output_vec[2])
    moterD.run(output_vec[3]) 